%根据规划完成分配
%输入：规划量(n*38)
%输出：[总步数，各小车路径点]
%PATH和Pallets2作为记录变化的表，随时保持更新
function [STEP,PATH]=CompeletAssign(batch)
global AgvData;
global Pallets2;
global Batch;

[m,n]=size(batch);
STEP=0;
Npath=10000;
PATH=zeros(19,Npath);
PATH(1:19,1:2)=AgvData(1:19,2:3);%初始位置
for i=1:1:m
    %%
    %分配车-绿点-蓝点
    STEP1=0;
    CarPoint=zeros(19,2);
    StoragePoint=zeros(19,2);
    
    %找到当前车的位置
    for j=1:1:19
        for k=1:1:Npath
            if  PATH(j,k)==0
                CarPoint(j,1:2)=PATH(j,k-2:k-1);
                break;
            end
        end
    end
    
    IlineSum=19;%默认一批19个货物
    for j=1:1:19 %本批任务共有几件货物
       if Batch(i,j)==0
          IlineSum=j-1; 
          break;
       end
    end
    
%     if i==4
%         aaaa=4;
%     end
    
    %找到货架号对应的货架坐标
    StoragePoint=[];
    for j=1:1:IlineSum
        for k=1:1:148
            if Pallets2(k,3)==Batch(i,j)
                StoragePoint(j,1:2)=Pallets2(k,1:2);
                Pallets2(k,3)=0; %更新Pallets2
                Pallets2(k,4)=0;
                break;
            end
        end
    end

 
    
    [MatrixOrder,STEP1]=AssignJob(CarPoint,StoragePoint); %完成车-绿-蓝分配
    
    
    %更新PATH
    for j=1:1:19 %PATH
        if MatrixOrder(j,2)~=0 %分配到任务的车更新路径
            MobileOrderP1=GetTrajectory([CarPoint(j,:);StoragePoint(MatrixOrder(j,2),:)]); %车到绿
            MobileOrderP2=GetTrajectory([StoragePoint(MatrixOrder(j,2),:);MatrixOrder(j,3:4)]);%绿到蓝
            [MMobileOrderP1,NMobileOrderP1]=size(MobileOrderP1);
            [MMobileOrderP2,NMobileOrderP2]=size(MobileOrderP2);
            if MMobileOrderP1>1 %原地踏步的不用更新
                for k=1:1:Npath %车-绿
                    if  PATH(j,k)==0
                        for p=1:1:MMobileOrderP1-1
                            PATH(j,k+2*p-2)=MobileOrderP1(p+1,1);
                            PATH(j,k+2*p-1)=MobileOrderP1(p+1,2);
                        end
                        break;
                    end
                end
            end
            if MMobileOrderP2>1 %原地踏步的不用更新
                for k=1:1:Npath %绿-蓝
                    if  PATH(j,k)==0
                        for p=1:1:MMobileOrderP2-1
                            PATH(j,k+2*p-2)=MobileOrderP2(p+1,1);
                            PATH(j,k+2*p-1)=MobileOrderP2(p+1,2);
                        end
                        break;
                    end
                end
            end
        end
    end
    
    %%
    %分配车（蓝点）-红/绿点
    STEP2=0;
    for j=1:1:19 %j为车
        if MatrixOrder(j,2)~=0 %分配到任务的继续下一步分配
            if batch(i,2*MatrixOrder(j,2))==1&&batch(i,2*MatrixOrder(j,2)-1)~=0  %去红点
                ToRedinput=MatrixOrder(j,3:4);%车在上一步分配的蓝点位置就是车现在的位置
                [STEP2RED,PATH2RED]=FindNearestRed(ToRedinput);
                [MPATH2RED,NPATH2RED]=size(PATH2RED);
                if MPATH2RED>1
                    for k=1:1:Npath %蓝-红PATH更新
                        if  PATH(j,k)==0
                            for p=1:1:MPATH2RED-1
                                PATH(j,k+2*p-2)=PATH2RED(p+1,1);
                                PATH(j,k+2*p-1)=PATH2RED(p+1,2);
                            end
                            break;
                        end
                    end
                end
                STEP2=STEP2+STEP2RED;
                %Pallets2没有动所以不更新
            elseif batch(i,2*MatrixOrder(j,2))==0 && batch(i,2*MatrixOrder(j,2)-1)~=0  %回绿点
                
                PalletsNone=[]; %空绿点的位置
                FlagPalletsNone=1;
                for k=1:1:148  %找空绿点
                    if Pallets2(k,3)==0
                        PalletsNone(FlagPalletsNone,1:2)=Pallets2(k,1:2);
                        FlagPalletsNone=FlagPalletsNone+1;
                    end
                end
                ToGreeninput=MatrixOrder(j,3:4);%车在上一步分配的蓝点位置就是车现在的位置
                [STEP2GREEN,PATH2GREEN]=FindNearestGreen(ToGreeninput,PalletsNone);
                [MPATH2GREEN,NPATH2GREEN]=size(PATH2GREEN);
                if MPATH2GREEN>1
                    for k=1:1:Npath %蓝-绿PATH更新
                        if  PATH(j,k)==0
                            for p=1:1:MPATH2GREEN-1
                                PATH(j,k+2*p-2)=PATH2GREEN(p+1,1);
                                PATH(j,k+2*p-1)=PATH2GREEN(p+1,2);
                            end
                            break;
                        end
                    end
                end
                STEP2=STEP2+STEP2GREEN;
                for k=1:1:148 %更新Pallets2
                    if  PATH2GREEN(end,1:2)==Pallets2(k,1:2)
                        Pallets2(k,3)=Batch(i,MatrixOrder(j,2));
                        Pallets2(k,4)=1;
                    end
                end
                
            end
        end
    end
    %%
     %计算总步数
    STEP=STEP+STEP1+STEP2;
    

end
    
   
end

